import math
import numpy as np

def readText(filename):
    arr = []
    with open(filename, 'r') as f:
        content = f.readlines()
        for i in range(len(content)):
            line = content[i].rstrip('\n')
            listLine = line.split(' ')
            listLine = [float(item) for item in listLine]
            arr.append(listLine)
    arr = np.array(arr, dtype=np.float64)
    # m = np.ones((arr.shape[0], 1))
    # arr = np.hstack((arr, m))  # 齐次坐标
    return arr

def normalize(x,y,z):
    sum = math.sqrt(x**2+y**2+z**2)
    m = x/sum
    n = y/sum
    p = z/sum
    return [m,n,p]

def createLine(n,n1):
    # n1为旋转轴向量，n2为旋转前后点方向
    arr = []
    (a0,b0,c0) = [i for i in n]
    for i in range(len(n1)):
        # print(np.dot(n,n1[i]))
        (a1,b1,c1) = [i for i in n1[i]]
        x = (c1*b0 - c0*b1)/(a0*b1 - a1*b0)
        y = (c1*a0 - c0*a1)/(b0*a1 - b1*a0)
        z = 1
        arr.append(normalize(x,y,z))
    return np.array(arr)

def computePoint(p1,p2,n,ANGLE,point0):
    # p1是旋转前点 p2 旋转后点 n为旋转轴 ANGLE旋转角 point0是髁突的参照点
    center = (p1+p2)/2
    lineVector = createLine(n,p1-p2)
    # return (center,lineVector)
    point = []
    for i in range(len(p1)):
        # t 步长 point1 旋转前点 point2 中间点 point3 迭代点 n0 重叠直线 n1 n2为计算角度的2个方向 nTemp为测试方向的向量
        n0 = lineVector[i]
        t = 0.2
        point1 = p1[i]
        point2 = center[i]
        point3 =  n0*t+point2
        point4 = p2[i]
        nTemp1 = point3 - point2
        nTemp2 = point0 - point2
        flag=1
        if np.dot(nTemp1,nTemp2)<0:
            flag = -1
            point3 =  flag*n0*t+point2
        n1 = point1 - point3
        n2 = point4 - point3
        len1 = math.sqrt(np.sum(n1**2))
        len2 = math.sqrt(np.sum(n2**2))
        angle = np.dot(n1,n2)/(len1*len2)
        # 求解弧度 弧度制
        angle = math.acos(angle)
        count = 0
        while count <20000 and math.fabs(ANGLE-angle)>0.001:
            t = t+0.01
            point3 =  flag*n0*t+point2
            n1 = point1 - point3
            n2 = point4 - point3
            len1 = math.sqrt(np.sum(n1 ** 2))
            len2 = math.sqrt(np.sum(n2 ** 2))
            angle = np.dot(n1, n2) / (len1 * len2)
            # 求解弧度 弧度制
            angle = math.acos(angle)
            count = count +1
        # print(count)
        point.append(point3)
    return (center,np.array(point))

##  由空间3维点拟合出一条直线
def linear_fitting_3D_points(points):
    # 表示矩阵中的值
    Sum_X = 0.0
    Sum_Y = 0.0
    Sum_Z = 0.0
    Sum_XZ = 0.0
    Sum_YZ = 0.0
    Sum_Z2 = 0.0

    for i in range(0, len(points)):
        xi = points[i][0]
        yi = points[i][1]
        zi = points[i][2]

        Sum_X = Sum_X + xi
        Sum_Y = Sum_Y + yi
        Sum_Z = Sum_Z + zi
        Sum_XZ = Sum_XZ + xi * zi
        Sum_YZ = Sum_YZ + yi * zi
        Sum_Z2 = Sum_Z2 + zi ** 2

    n = len(points)  # 点数
    den = n * Sum_Z2 - Sum_Z * Sum_Z  # 公式分母
    k1 = (n * Sum_XZ - Sum_X * Sum_Z) / den
    b1 = (Sum_X - k1 * Sum_Z) / n
    k2 = (n * Sum_YZ - Sum_Y * Sum_Z) / den
    b2 = (Sum_Y - k2 * Sum_Z) / n

    return (k1, b1, k2, b2)

def get_uvwsita(M):
    t1 = M[2][1] - M[1][2]
    t2 = M[0][2] - M[2][0]
    t3 = M[1][0] - M[0][1]
    u = 1/math.sqrt(1+(t2**2+t3**2)/(t1**2))
    v = u * t2 / t1
    w = u * t3 / t1
    sita = math.acos((M[0][0] - u**2)/(1-u**2))
    return (np.array((u,v,w)),sita)

def compute_axis(path1,path2,r_matrix):
    # 旋转前的点
    p1 = readText(path1)
    # 旋转后的点
    p2 = readText(path2)

    n,ANGLE = get_uvwsita(r_matrix)

    # 髁突的参考点 为的是 用角度查找点的时候 不会迭代错方向
    point0 = np.array([-42.7812, 49.2365, 28.8234])

    # center是旋转前后的中间的 points是最后迭代出来的点
    center,points = computePoint(p1, p2, n, ANGLE, point0)
    # 拟合直线
    return linear_fitting_3D_points(points)




